#include "servo.h"
#include "stm32f10x.h"

static int16_t dir_error = 0; //Dirmid correction value.

void Servo_Init()
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_OCInitTypeDef TIM_OCInitStructure;
	GPIO_InitTypeDef GPIO_InitStruct;

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_1;
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;

	GPIO_Init(GPIOA, &GPIO_InitStruct);

	TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStruct.TIM_Period = 1999;
	TIM_TimeBaseInitStruct.TIM_Prescaler = 119;
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStruct);

	TIM_ARRPreloadConfig(TIM2, ENABLE);

	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

	TIM_OC2Init(TIM2, &TIM_OCInitStructure);

	TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);

	TIM_Cmd(TIM2, ENABLE);

	Servo_DirSet(900);
}

void Servo_DirSet(uint16_t direction)
{
	TIM_SetCompare2(TIM2, (uint16_t)(direction * 1200 / 1800 + 300 + dir_error)); //Convert to compare value.//300Hz
																				  //TIM_SetCompare2(TIM2, (uint16_t)(direction * 200 / 1800 + 50 + dir_error)); //Convert to compare value.//300Hz
}

void Servo_MidDirSet(int16_t direction)
{
	dir_error = direction;
}
